/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package br.ufrn.engcomp.mpc.util;

import java.util.ArrayList;
import java.util.List;
import org.apache.commons.math.linear.Array2DRowRealMatrix;
import org.apache.commons.math.linear.LUDecompositionImpl;
import org.apache.commons.math.linear.RealMatrix;
import org.apache.commons.math.linear.RealVector;

/**
 *
 * @author Felipe
 */
public class MathCommonsUtils {

    // Equivalente a funcao "zeros" do Matlab
    public static RealMatrix zeros(int nRows, int nColumns) {
        if (nRows > 0 && nColumns > 0) {
            RealMatrix zeros = new Array2DRowRealMatrix(nRows, nColumns);

            for (int i = 0; i < nRows; i++) {
                for (int j = 0; j < nColumns; j++) {
                    zeros.setEntry(i, j, 0);
                }
            }

            return zeros;
        } else {
            return null;
        }
    }

    // Equivalente a funcao "ones" do Matlab
    public static RealMatrix ones(int nRows, int nColumns) {
        if (nRows > 0 && nColumns > 0) {
            RealMatrix ones = new Array2DRowRealMatrix(nRows, nColumns);

            for (int i = 0; i < nRows; i++) {
                for (int j = 0; j < nColumns; j++) {
                    ones.setEntry(i, j, 1);
                }
            }

            return ones;
        } else {
            return null;
        }
    }

    public static RealMatrix eye(int order) {
        if (order > 0) {
            RealMatrix eye = new Array2DRowRealMatrix(order, order);

            for (int i = 0; i < order; i++) {
                for (int j = 0; j < order; j++) {
                    if (i == j) {
                        eye.setEntry(i, j, 1);
                    } else {
                        eye.setEntry(i, j, 0);
                    }
                }
            }

            return eye;
        } else {
            return null;
        }
    }

    public static RealMatrix concatenateRow(RealMatrix... allMatrices) {
        RealMatrix[] matrices = removeNullMatrices(allMatrices);

        if (matrices.length > 0) {
            int rowDimension = 0;
            int columnDimension = matrices[0].getColumnDimension();

            for (RealMatrix matrix : matrices) {
                rowDimension += matrix.getRowDimension();

                if (matrix.getColumnDimension() != columnDimension) {
                    throw new IllegalArgumentException("Matrices aren't all same column size!");
                }
            }

            RealMatrix concMatrix = new Array2DRowRealMatrix(rowDimension, columnDimension);

            int pointer = 0;

            for (RealMatrix matrix : matrices) {
                concMatrix.setSubMatrix(matrix.getData(), pointer, 0);

                pointer += matrix.getRowDimension();
            }

            return concMatrix;
        } else {
            return null;
        }
    }

    public static RealMatrix concatenateColumn(RealMatrix... allMatrices) {
        RealMatrix[] matrices = removeNullMatrices(allMatrices);

        if (matrices.length > 0) {
            int rowDimension = matrices[0].getRowDimension();
            int columnDimension = 0;

            for (RealMatrix matrix : matrices) {
                columnDimension += matrix.getColumnDimension();

                if (matrix.getRowDimension() != rowDimension) {
                    throw new IllegalArgumentException("Matrices aren't all same row size!");
                }
            }

            RealMatrix concMatrix = new Array2DRowRealMatrix(rowDimension, columnDimension);

            int pointer = 0;

            for (RealMatrix matrix : matrices) {
                concMatrix.setSubMatrix(matrix.getData(), 0, pointer);

                pointer += matrix.getColumnDimension();
            }

            return concMatrix;
        } else {
            return null;
        }
    }

    public static RealMatrix convertColumnVectorToMatrix(RealVector vector) {
        RealMatrix matrix = new Array2DRowRealMatrix(vector.getDimension(), 1);

        matrix.setColumnVector(0, vector);

        return matrix;
    }

    public static RealMatrix convertRowVectorToMatrix(RealVector vector) {
        RealMatrix matrix = new Array2DRowRealMatrix(1, vector.getDimension());

        matrix.setRowVector(0, vector);

        return matrix;
    }

//    public static RealMatrix convertVectorToMatrix(RealVector vector) {
//        RealMatrix matrix = new Array2DRowRealMatrix(1, vector.getDimension());
//        
//        if (vector.get)
//
//        matrix.setRowVector(0, vector);
//
//        return matrix;
//    }

    public static RealVector convertMatrixToColumnVector(RealMatrix matrix) {
        return matrix.getColumnVector(0);
    }

    public static RealVector convertMatrixToRowVector(RealMatrix matrix) {
        return matrix.getRowVector(0);
    }

    private static RealMatrix[] removeNullMatrices(RealMatrix[] matrices) {
        List<RealMatrix> notNullMatrices = new ArrayList<RealMatrix>();

        for (RealMatrix matrix : matrices) {
            if (matrix != null) {
                notNullMatrices.add(matrix);
            }
        }

        return (RealMatrix[]) notNullMatrices.toArray(new RealMatrix[notNullMatrices.size()]);
    }

    public static RealVector solveAXb(RealMatrix A, RealMatrix b) {
        // Solução do sistema:
        // X = inv(sysA) * sysB
        RealMatrix invA = new LUDecompositionImpl(A).getSolver().getInverse();
        RealMatrix X = invA.multiply(b);

        return convertMatrixToColumnVector(X);
    }

    public static int getStartPointer(int i, int length) {
        return i * length;
    }

    public static int getEndPointer(int startPointer, int length) {
        return (startPointer + length - 1);
    }
    
    public static RealMatrix generateIdentityFromMatrix(RealMatrix matrix, int length) {
        int nRows = matrix.getRowDimension();
        int nColumns = matrix.getColumnDimension();

        RealMatrix identityFromMatrix = MathCommonsUtils.zeros(length * nRows, length * nColumns);

        int rStartPointer;
        int cStartPointer;

        for (int i = 0; i < length; i++) {
            rStartPointer = getStartPointer(i, nRows);
            cStartPointer = getStartPointer(i, nColumns);

            identityFromMatrix.setSubMatrix(matrix.getData(), rStartPointer, cStartPointer);
        }
        
        return identityFromMatrix;
    }

    public static RealMatrix generateRowFromVector(RealVector vector, int length) {
        RealMatrix matrix = MathCommonsUtils.convertRowVectorToMatrix(vector);
        int nRows = matrix.getRowDimension();
        int nColumns = matrix.getColumnDimension();

        RealMatrix rowFromMatrix = MathCommonsUtils.zeros(nRows, length * nColumns);

        int cStartPointer;

        for (int i = 0; i < length; i++) {
            cStartPointer = getStartPointer(i, nColumns);

            rowFromMatrix.setSubMatrix(matrix.getData(), 0, cStartPointer);
        }
        
        return rowFromMatrix;
    }

    public static RealMatrix generateColumnFromVector(RealVector vector, int length) {
        RealMatrix matrix = MathCommonsUtils.convertColumnVectorToMatrix(vector);
        int nRows = matrix.getRowDimension();
        int nColumns = matrix.getColumnDimension();

        RealMatrix columnFromMatrix = MathCommonsUtils.zeros(length * nRows, nColumns);

        int rStartPointer;

        for (int i = 0; i < length; i++) {
            rStartPointer = getStartPointer(i, nRows);

            columnFromMatrix.setSubMatrix(matrix.getData(), rStartPointer, 0);
        }
        
        return columnFromMatrix;
    }
    
    public static void print(RealMatrix matrix, String name) {
        StringBuilder sb = new StringBuilder();
        String value;
        
        for (int i=0; i<matrix.getRowDimension(); i++) {
            sb.append("|");
            
            for (int j=0; j<matrix.getColumnDimension(); j++) {
                value = String.valueOf(matrix.getEntry(i, j));
                
                sb.append(value);
                
                if (j < matrix.getColumnDimension() - 1) {
                    if (value.length() < 7) {
                        sb.append("\t\t");
                    } else {
                        sb.append("\t");
                    }
                }
            }
            
            sb.append("|\n");
        }
        
        System.out.println(name + ":\n" + sb.toString());
    }

}
